/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2025 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "oled.h"
#include "Control.h"

//---------------------mpu6050begin-------------
#include "mpu6050.h"
#include "IIC.h"
#include "stdio.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
//---------------------mpu6050end---------------

//---------------------上下位机通信begin------------
/*
启动DMA接收:HAL_UART_Receive_DMA(&huart3, JudgeReceiveBuffer, 5);
DMA发送数据：usartSendData(0.0,0.0,311,1);对应左侧轮速，右侧轮速，偏航角，指定一个字节发送
*/
#include "serial.h"
//--------------------上下位机通信end------------

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

uint8_t RX_dat; // 定义接收蓝牙（串口4）数据变量

float pitch, roll, yaw;    // 欧拉角
short aacx, aacy, aacz;    // 加速度传感器原始数据
short gyrox, gyroy, gyroz; // 陀螺仪原始数据
// float temp;                // 温度
//  while(mpu_dmp_get_data(&pitch, &roll, &yaw));//必须要用while等待，才能读取成功
//  MPU_Get_Accelerometer(&aacx,&aacy, &aacz);		//得到加速度传感器数据
//  MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);		//得到陀螺仪数据
//  temp=MPU_Get_Temperature();						//得到温度信息

uint8_t received_data;
/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{

  /* USER CODE BEGIN 1 */
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_I2C1_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_TIM8_Init();
  MX_USART2_UART_Init();
  MX_TIM6_Init();
  MX_UART4_Init();
  MX_USART3_UART_Init();
  MX_UART5_Init();
  MX_TIM1_Init();
  /* USER CODE BEGIN 2 */
  //-----------------OLED初始化begin-----------------
  OLED_Init();
  OLED_Clear();
  //-----------------OLED初始化end-------------------

  //-------------------MPU6050初始化begin-------------
  MPU_Init();     // MPU6050初始化
  mpu_dmp_init(); // dmp初始化
  //-------------------MPU6050初始化end---------------

  // OLED_ShowString(0, 0, "Hello World!", 16, 0);

  /*开启定时器6的中断，每隔40ms中断一次*/
  HAL_TIM_Base_Start_IT(&htim6);
  HAL_UART_Receive_IT(&huart4, &RX_dat, 1);       // 开启接收中断
  HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL); // 开启编码器模式
  HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); // 开启编码器模式
  HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); // 开启编码器模式
  HAL_TIM_Encoder_Start(&htim8, TIM_CHANNEL_ALL); // 开启编码器模式
  /*开启TIM1的PWM通道，不分频，自动重装值为7200，PWM频率为10khz，占空比最大为7200*/
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);

  // HAL_UART_Receive_DMA(&huart3, JudgeReceiveBuffer, reBiggestSize);
  HAL_UART_Receive_DMA(&huart3, &received_data, 6);

  // OLED_Showdecimal(0,2,LF_PID.P,4,1,16,0);

  // right_motor_forward(1);
  printf("初始化成功！\r\n");
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */
    /* USER CODE BEGIN 3 */
    while (mpu_dmp_get_data(&pitch, &roll, &yaw))
      ; // 必须要用while等待，才能读取成功
  }
  /* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
   * in the RCC_OscInitTypeDef structure.
   */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
   */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
/*
定时器6中断回调函数
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if (htim == (&htim6))
  {
    RF_PID.Actual_maichong = (int16_t)__HAL_TIM_GetCounter(&htim2); // 获取电机1的计数值
    LF_PID.Actual_maichong = (int16_t)__HAL_TIM_GetCounter(&htim3); // 获取电机2的计数值
    RB_PID.Actual_maichong = (int16_t)__HAL_TIM_GetCounter(&htim4); // 获取电机3的计数值
    LB_PID.Actual_maichong = (int16_t)__HAL_TIM_GetCounter(&htim8); // 获取电机4的计数值
    __HAL_TIM_SetCounter(&htim2, 0);                                // 清空计数值
    __HAL_TIM_SetCounter(&htim3, 0);                                // 清空计数值
    __HAL_TIM_SetCounter(&htim4, 0);                                // 清空计数值
    __HAL_TIM_SetCounter(&htim8, 0);                                // 清空计数值
    static float aaa = 0;
    aaa++;
    OLED_Showdecimal(10, 0, aaa, 4, 1, 16, 0);
    // OLED_Showdecimal(60,2, RF_PID.Actual_maichong, 4, 2, 16, 0);
    // OLED_Showdecimal(60,2, LF_PID.Actual_maichong, 4, 1, 16, 0);
    // OLED_Showdecimal(60,4, RB_PID.Actual_maichong, 4, 1, 16, 0);
    // OLED_Showdecimal(60,6, LB_PID.Actual_maichong, 4, 1, 16, 0);
    float LF_actual_speed = (float)(LF_PID.Actual_maichong) * C_cm / (maichong * tim6_Time); // 左侧电机实际速度
    float RF_actual_speed = (float)(RF_PID.Actual_maichong) * C_cm / (maichong * tim6_Time); // 右侧电机实际速度
    float LB_actual_speed = (float)(LB_PID.Actual_maichong) * C_cm / (maichong * tim6_Time); // 左后电机实际速度
    float RB_actual_speed = (float)(RB_PID.Actual_maichong) * C_cm / (maichong * tim6_Time); // 右后电机实际速度
    usartSendData((short)((LF_actual_speed+LB_actual_speed)/2),\
                  (short)((RF_actual_speed+RB_actual_speed)/2),\
                  (short)yaw,1);//发送消息给上位机,速度的单位为cm/s
    Motor_Set_Speed(1, &RF_target_speed); // 设置速度
    Motor_Set_Speed(2, &LF_target_speed);
    Motor_Set_Speed(3, &RB_target_speed);
    Motor_Set_Speed(4, &LB_target_speed);

    // printf("%d,%d,%f,%d,%f,%f,%f\r\n", RF_PID.Target_maichong, RF_PID.Actual_maichong,RF_target_speed,RF_PID.Output,RF_PID.P,RF_PID.I,RF_PID.D);
    // printf("%d,%d,%f,%d,%f,%f,%f\r\n", LF_PID.Target_maichong, LF_PID.Actual_maichong,LF_target_speed,LF_PID.Output,LF_PID.P,LF_PID.I,LF_PID.D);
    // printf("%d,%d,%f,%d,%f,%f,%f\r\n", RB_PID.Target_maichong, RB_PID.Actual_maichong,RB_target_speed,RB_PID.Output,RB_PID.P,RB_PID.I,RB_PID.D);
    // printf("%d,%d,%f,%d,%f,%f,%f\r\n", LB_PID.Target_maichong, LB_PID.Actual_maichong,LB_target_speed,LB_PID.Output,LB_PID.P,LB_PID.I,LB_PID.D);
  }
}
/*
串口回调函数
*/
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) // 串口中断服务函数
{
  // RX DMA接收完成回调函数
  if (huart->Instance == USART3)
  {
    // 处理接收到的数据
    HAL_UART_Receive_DMA(&huart3, JudgeReceiveBuffer, reBiggestSize);
    // HAL_UART_Receive_DMA(&huart3, &received_data, 6);
    // printf("接收到数据：%x\r\n", received_data);
    if (received_data == 0x01)    //前进
    {
      LF_target_speed = 0.1; // 左侧电机目标速度
      LB_target_speed = 0.1; // 左后电机目标速度
      RF_target_speed = 0.1; // 右侧电机目标速度
      RB_target_speed = 0.1; // 右后电机目标速度
    }
    else if (received_data == 0x02) //后退
    {
      LF_target_speed = -0.1; // 左侧电机目标速度
      LB_target_speed = -0.1; // 左后电机目标速度
      RF_target_speed = -0.1; // 右侧电机目标速度
      RB_target_speed = -0.1; // 右后电机目标速度
    }
    else if (received_data == 0x03) //左转
    {
      LF_target_speed = -0.1; // 左侧电机目标速度
      LB_target_speed = -0.1; // 左后电机目标速度
      RF_target_speed = 0.1; // 右侧电机目标速度
      RB_target_speed = 0.1; // 右后电机目标速度
    }
    else if (received_data == 0x04) //右转
    {      
      LF_target_speed = 0.1; // 左侧电机目标速度
      LB_target_speed = 0.1; // 左后电机目标速度
      RF_target_speed = -0.1; // 右侧电机目标速度
      RB_target_speed = -0.1; // 右后电机目标速度
    }
    else if(received_data == 0x05)
    {
      LF_target_speed = 0; // 左侧电机目标速度
      LB_target_speed = 0; // 左后电机目标速度
      RF_target_speed = 0; // 右侧电机目标速度
      RB_target_speed = 0; // 右后电机目标速度
    }

    JudgeBuffReceive(JudgeReceiveBuffer);
    LF_target_speed = (float)chassis_ctrl.leftSpeedSet/100;//转换速度单位为m/s
    LB_target_speed = (float)chassis_ctrl.leftSpeedSet/100;
    RF_target_speed = (float)chassis_ctrl.rightSpeedSet/100;
    RB_target_speed = (float)chassis_ctrl.rightSpeedSet/100;
    OLED_Showdecimal(0,2,LF_target_speed,3,2,16,0);
    OLED_Showdecimal(0,4,RF_target_speed,3,2,16,0);
  }
  if (huart == &huart4) // 判断是否为蓝牙串口
  {
    if (RX_dat == 0x01) // P增大
    {
      RF_PID.P += 1;
      LF_PID.P += 1;
      RB_PID.P += 1;
      LB_PID.P += 1;
    }
    else if (RX_dat == 0x2) // P减小
    {
      RF_PID.P -= 1;
      LF_PID.P -= 1;
      RB_PID.P -= 1;
      LB_PID.P -= 1;
    }
    else if (RX_dat == 0x03) // I增大
    {
      RF_PID.I += 1;
      LF_PID.I += 1;
      RB_PID.I += 1;
      LB_PID.I += 1;
    }
    else if (RX_dat == 0x04) // I减小
    {
      RF_PID.I -= 1;
      LF_PID.I -= 1;
      RB_PID.I -= 1;
      LB_PID.I -= 1;
    }
    else if (RX_dat == 0x05) // D增大
    {
      RF_PID.D += 1;
      LF_PID.D += 1;
      RB_PID.D += 1;
      LB_PID.D += 1;
    }
    else if (RX_dat == 0x06) // D减小
    {
      RF_PID.D -= 1;
      LF_PID.D -= 1;
      RB_PID.D -= 1;
      LB_PID.D -= 1;
    }
    else if (RX_dat == 0x07) // 目标速度增大0.1
    {
      RF_target_speed += 0.1;
      LF_target_speed += 0.1;
      RB_target_speed += 0.1;
      LB_target_speed += 0.1;
    }
    else if (RX_dat == 0x08) // 目标速度减小0.1
    {
      RF_target_speed -= 0.1;
      LF_target_speed -= 0.1;
      RB_target_speed -= 0.1;
      LB_target_speed -= 0.1;
    }
    HAL_UART_Receive_IT(&huart4, &RX_dat, 1); // 每次接收以后都要再次开启接收中断
  }
}
/*
常用函数
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 2000);//电机1的PWM
 HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);//LED1 on

 RB_PID.Actual = __HAL_TIM_GetCounter(&htim2);//获取电机1的计数值
*/
/* USER CODE END 4 */

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
